// Copyright 2020 Poofee (https://github.com/Poofee)
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// ------------------------------------------------------------------------
/*****************************************************************************
 *                                                                           *
 *  File:    relay3d.cpp                                                     *
 *                                                                           *
 *****************************************************************************
 *                                                                           *
 *  Authors: Poofee                                                          *
 *  Email:   poofee@qq.com                                                   *
 *  Address: https://github.com/Poofee                                       *
 *  Date:    2020年07月15日                                                   *
 *                                                                           *
 *****************************************************************************/
#include "relay3d.h"

#include "libstructure.h"
#include "tetralheral.h"

#include "SuperLU_MT.h"

Relay3D::Relay3D() {

}



/*!
 * @brief      三维重分网函数
 * @author     Poofee
 * @date       2020年9月18日
 * @param[out] 
 * @param[in]  int timestep  
 * @return     void  
**/
void Relay3D::remesh(int timestep) {
    double s;

    if (motion.type == STRAIGHTMOTION) {
		/** 直动式 **/
        s = Ddisplacements[timestep]/unitRatio;
		remesh3DParallel(s*motion.v[0], s*motion.v[1], s*motion.v[2]);
	} else if (motion.type == ROTATEMOTION) {
		/** 旋转式 **/
        s = Ddisplacements[timestep];
		remesh3DRotate(s, motion.p[0], motion.p[1], motion.p[2], motion.v[0], motion.v[1], motion.v[2]);
	}
}


/*!
 * @brief      查找空气单元位置
 * @author     Poofee
 * @date       2020年10月27日
 * @param[out] 
 * @return     void  
**/
void Relay3D::findAir() {
	int i1;
	for (int i = 0; i < num_tet; ++i) {
		i1 = num_ele - num_tet + i;
		if (pmeshele[i1].geometry_tag == tag_air) {
			air_position = i;
			break;
		}
	}
}

/*!
 * @brief      三维当中，根据线圈参数生成每个节点的电流方向。
 * @author     Poofee
 * @date       2020年9月18日
 * @param[out] 
 * @return     void  
**/
void Relay3D::makeJ() {
	for (int i = 0; i < num_pts; ++i) {
		pmeshnode[i].J[0] = 0;
		pmeshnode[i].J[1] = 0;
		pmeshnode[i].J[2] = 0;
	}
	for (int i_tet = 0; i_tet < num_tet; ++i_tet) {
		CElement* ele = pmeshele + num_ele - num_tet + i_tet;
		CMaterial* mat = getMaterial(ele->geometry_tag);

		/** 含有线圈 **/
		if (mat->coil) {
			
			if (mat->coil->type == CICULARCOIL) {
				/** CicularCoil **/
				makeJCircularCoil(ele, mat);
			} else if (mat->coil->type == RECTANGULARCOIL) {
				/** RectangularCoil **/
				makeJRectangularCoil(ele, mat);
			}
		}
	}/** end of for **/
}


/*!
 * @brief      计算圆形线圈的电流方向向量
 * @author     Poofee
 * @date       2020年9月18日
 * @param[out] 
 * @param[in]  CElement * ele  
 * @param[in]  CMaterial * mat  
 * @return     void  
**/
void Relay3D::makeJCircularCoil(CElement* ele, CMaterial* mat) {
	int    x = 0, y = 0, z = 0;
	Coil* coil = mat->coil;

	/** 判断是在哪个轴上 **/
	if (coil->xyz == 0) {
		x = 1; y = 2; z = 0;
	} else if (coil->xyz == 1) {
		x = 2; y = 0; z = 1;
	} else if (coil->xyz == 2) {
		x = 0; y = 1; z = 2;
	}
	/** 对每个点进行计算 **/
	for (int i = 0; i < 4; ++i) {
		int    ii = ele->n[i];
		double ap[3];
		double crd[3];
		double theta = 0.0;

		crd[0] = pmeshnode[ii].x;
		crd[1] = pmeshnode[ii].y;
		crd[2] = pmeshnode[ii].z;

		/** 计算半径 **/
		ap[0] = crd[x] - coil->center[x];
		ap[1] = crd[y] - coil->center[y];
		ap[2] = crd[z];

		/* r0 <= l <= r1 */
		double l = 0.0;
		for (int j = 0; j < 2; j++) {
			l += ap[j] * ap[j];
		}
		l = sqrt(l);
		if ((l < coil->r0) || ((coil->r0 + coil->width) < l)) {
			//continue;
		}

		/** 计算角度 **/
		theta = atan2(ap[1], ap[0]);
		/* make Jo */
		pmeshnode[ii].J[x] = -sin(theta);
		pmeshnode[ii].J[y] = cos(theta);
		pmeshnode[ii].J[z] = 0.0;
	}
}



/*!
 * @brief      计算圆角矩形线圈的电流方向向量。只考虑线圈轴与坐标轴平行的情况。只考虑线圈与各个平面无偏角
 * @author     Poofee
 * @date       2020年9月18日
 * @param[out] 
 * @param[in]  CElement * ele  
 * @param[in]  CMaterial * mat  
 * @return     void  
**/
void Relay3D::makeJRectangularCoil(CElement* ele, CMaterial* mat) {
	int    x = 0, y = 0, z = 0;
	Coil* coil = mat->coil;

	/** 判断是在哪个轴上 **/
	if (coil->xyz == 0) {
		x = 1; y = 2; z = 0;
	} else if (coil->xyz == 1) {
		x = 2; y = 0; z = 1;
	} else if (coil->xyz == 2) {
		x = 0; y = 1; z = 2;
	}
	/** 判断点在哪个区域，一共划分9个区域 **/
	/** 对每个点进行计算 **/
	for (int i = 0; i < 4; ++i) {
		int    ii = ele->n[i];
		/** 判断是在圆内还是长方形内 **/
		double ap[3];
		double crd[3];
		double center[3];
		double theta = 0.0;

		crd[0] = pmeshnode[ii].x;
		crd[1] = pmeshnode[ii].y;
		crd[2] = pmeshnode[ii].z;

		/** 计算半径 **/
		ap[0] = crd[x] - coil->center[x];
		ap[1] = crd[y] - coil->center[y];
		ap[2] = crd[z];

		if (std::abs(ap[0]) >= 0.5*coil->xWidth && std::abs(ap[1]) >= 0.5*coil->yWidth) {
            if (ap[0] > 0 && ap[1] > 0) {
                /** 第1象限 **/
				center[x] = coil->center[x] + 0.5*coil->xWidth;
				center[y] = coil->center[y] + 0.5*coil->yWidth;
            } else if (ap[0] < 0 && ap[1] > 0) {
                /** 第2象限 **/
				center[x] = coil->center[x] + 0.5*coil->xWidth;
				center[y] = coil->center[y] - 0.5*coil->yWidth;
            } else if (ap[0] < 0 && ap[1] < 0) {
                /** 第3象限 **/
				center[x] = coil->center[x] - 0.5*coil->xWidth;
				center[y] = coil->center[y] - 0.5*coil->yWidth;
            } else if (ap[0] > 0 && ap[1] < 0) {
                /** 第4象限 **/
				center[x] = coil->center[x] + 0.5*coil->xWidth;
				center[y] = coil->center[y] - 0.5*coil->yWidth;
			} else {
				printf("Relay3D::makeJRectangularCoil:0 Should't be here!\n");
			}
			ap[0] = crd[x] - center[x];
			ap[1] = crd[y] - center[y];
			ap[2] = crd[z];
			/** 计算角度 **/
			theta = atan2(ap[1], ap[0]);
			/* make Jo */
			pmeshnode[ii].J[x] = -sin(theta);
			pmeshnode[ii].J[y] = cos(theta);
			pmeshnode[ii].J[z] = 0.0;
		} else {
			if (crd[x] >= coil->center[x] + 0.5*coil->xWidth && crd[x] <= coil->center[x] + 0.5*coil->xWidth + coil->width) {
				pmeshnode[ii].J[x] = 0;
				pmeshnode[ii].J[y] = 1.0;
				pmeshnode[ii].J[z] = 0;
			} else if (crd[x] <= coil->center[x] - 0.5*coil->xWidth && crd[x] >= coil->center[x] - 0.5*coil->xWidth - coil->width) {
				pmeshnode[ii].J[x] = 0;
				pmeshnode[ii].J[y] = -1.0;
				pmeshnode[ii].J[z] = 0;
			} else if (crd[y] >= coil->center[y] + 0.5*coil->yWidth && crd[y] <= coil->center[y] + 0.5*coil->yWidth + coil->width) {
				pmeshnode[ii].J[x] = -1.0;
				pmeshnode[ii].J[y] = 0;
				pmeshnode[ii].J[z] = 0;
			} else if (crd[y] <= coil->center[y] - 0.5*coil->yWidth && crd[y] >= coil->center[y] - 0.5*coil->yWidth - coil->width) {
				pmeshnode[ii].J[x] = 1.0;
				pmeshnode[ii].J[y] = 0;
				pmeshnode[ii].J[z] = 0;
			} else {
				printf("Relay3D::makeJRectangularCoil:1 Should't be here!\n");
			}
		}
	}
}



/*!
 * @brief      三维分配内存。
 * @author     Poofee
 * @date       2020年9月18日
 * @param[out] 
 * @return     void  
**/
void Relay3D::allocateMemory() {
    int size = 10 * 10;
	locs.resize(2, size * num_tet);
	vals.resize(1, size * num_tet);
    //bbJz.resize(num_edges); bbJz.zeros();
    node_reorder.resize(num_edges + num_pts);
    node_pos.resize(num_edges + num_pts);
    bn.resize(num_edges);bn.zeros();
    A.resize(num_edges + num_pts); A.zeros();
}



/*!
 * @brief      施加Dirichlet边界条件。
 * @author     Poofee
 * @date       2020年9月18日
 * @param[out] 
 * @return     void  
**/
void Relay3D::setBoundary() {
	int node_bdr = boundaryPoints.size();
	int node_all = allPoints.size();
	int edge_bdr = boundaryEdges.size();
	int edge_all = allEdges.size();

	/** 标记边界点 **/
	for (int i2 = 0; i2 < num_pts; ++i2) {
		pmeshnode[i2].bdr = 3;
	}
	for (int i2 = 0; i2 < node_all; ++i2) {
		pmeshnode[allPoints[i2]].bdr = 0;
	}
	for (int i2 = 0; i2 < node_bdr; ++i2) {
		pmeshnode[boundaryPoints[i2]].bdr = 3;
	}
	/** 标记边界棱 **/
	for (int i = 0; i < edge_bdr; ++i) {
		FEMedge e = boundaryEdges[i];
		int start = e.start;
		int end = e.end;
		std::unordered_map<std::pair<int, int>, int, pair_hash>::iterator it;
		if (start < end) {
			it = alledgeshash.find(std::pair<int, int>(start, end));
		} else {
			it = alledgeshash.find(std::pair<int, int>(end, start));
		}

		if (it != alledgeshash.end()) {
			allEdges[it->second].bdr = 1;
		} else {
			printf("Error in void Relay3D::setBoundary()\n");
			return;
		}
	}
	/** 重编号 **/
	edge_bdr = 0;
	for (int ebdr = 0; ebdr < edge_all; ++ebdr) {
		if (allEdges[ebdr].bdr == 1) {
			++edge_bdr;
			node_reorder(edge_all + num_pts - edge_bdr) = ebdr;
			node_pos(ebdr) = edge_all + num_pts - edge_bdr;
			allEdges[ebdr].A = 0.0;
			A(ebdr) = 0.0;
		} else {
			node_reorder(ebdr - edge_bdr) = ebdr;
			node_pos(ebdr) = ebdr - edge_bdr;
		}
	}
	node_bdr = 0;
	for (int ibdr = 0; ibdr < num_pts; ++ibdr) {
		if (pmeshnode[ibdr].bdr == 3) {
			++node_bdr;
			node_reorder(edge_all - edge_bdr + num_pts - node_bdr) = edge_all + ibdr;
			node_pos(edge_all + ibdr) = edge_all - edge_bdr + num_pts - node_bdr;
			pmeshnode[ibdr].A = 0.0;
			A(edge_all - edge_bdr + ibdr) = 0.0;
		} else {
			node_reorder(edge_all - edge_bdr + ibdr - node_bdr) = edge_all + ibdr;
			node_pos(edge_all + ibdr) = edge_all - edge_bdr + ibdr - node_bdr;
		}
	}
}


/*!
 * @brief      计算三维静态电磁场，牛顿迭代法。
 * @author     Poofee
 * @date       2020年8月26日
 * @param[out] 
 * @return     void  
**/
void Relay3D::MagnetoStatic3DNR() {
    printf("\nStart static simulation using NR...\n");
    double t0, t1, t2;
	t0 = SuperLU_timer_();
	int timesize = timesteps.size();
	for (int i_time = 0; i_time < timesize; ++i_time) {
        printCurrentTime();
        printf("Static step %d/%d\n", i_time, timesize);

		/** remesh，要使用增量位移，向下为负，向上为正 **/
		remesh(i_time);

		/** 读取分网 **/
		loadMesh();
        unitConvert(unitRatio);

		/** 计算棱自由度 **/
		findBoundaryFaces3D(-1);
		findBoundaryEdges3D(-1);
        if(0) {
            char tmpfn[256];
            sprintf(tmpfn, "%s%sbnd.vtk", fileName, prefixName);
            testBoundaryResults(tmpfn);
            if (checkConnectivityMatrix()) {
                printf("Connectivity Matrix is right.\n");
            } else {
                printf("Connectivity Matrix is wrong.\n");
            }
        }

		makeJ();
        if(0) {
            char tmpfn[256];
            sprintf(tmpfn, "%s%sJ.vtk", fileName, prefixName);
            writeCurrentDensityField(tmpfn);
        }
        int edge_dof = num_edges - boundaryEdges.size();
		int num_dof = edge_dof + allPoints.size() - boundaryPoints.size();

        printf("DoF %d\n", num_dof);
		double* unknown_b = (double*)calloc(num_dof, sizeof(double));

		allocateMemory();
		setBoundary();

		double error;
		bn.zeros();
		bool LinearFlag = true;
		/** 非线性迭代求解 **/
        SimulationTime[current_step] = 0;
		for (int non_iter = 0; non_iter < MAX_NONLINEARSTEPS; ++non_iter) {
            t2 = SuperLU_timer_();
            printCurrentTime();
            printf("Time step (%d/%d)\tNewton's step (%d/%d)\n", i_time, timesize, non_iter, MAX_NONLINEARSTEPS);
            int pos = 0;
			bn.zeros();
			for (int i_tet = 0; i_tet < num_tet; ++i_tet) {
				int    i, j, k, ii;
				double x[ap_elem], y[ap_elem], z[ap_elem];
				double D;
				double nu = 0;
				double tetg[ap_elem];
				double l[mp_elem];
				int    si[mp_elem];
				double xi[mp_elem], yi[mp_elem], zi[mp_elem];
				double xy[mp_elem], yz[mp_elem], zx[mp_elem];
				double X[mp_elem], Y[mp_elem], Z[mp_elem];
				double Fx[mp_elem], Fy[mp_elem], Fz[mp_elem];
				double b[ap_elem], c[ap_elem], d[ap_elem];
				double rr[mp_elem][mp_elem], gs[mp_elem][ap_elem], sg[ap_elem][mp_elem];
				//int    indRow[nd_elem];
				double ae[nd_elem*nd_elem];
				double fe[nd_elem];
				double rotA[DIM];
				double B2 = 0.0;
				int    szRow = nd_elem;
				double Joe[ap_elem][DIM];
				//double Ihe[ap_elem];

				double be_JoA[mp_elem];
				//double be_gs[mp_elem];
				double be[nd_elem];
				double dnu = 0.0;
				double xe[mp_elem];

				/** 初始化 **/
				for (i = 0, k = 0; i < nd_elem; i++) {
					for (j = 0; j < nd_elem; j++, k++)
						ae[k] = 0.0;
					fe[i] = 0.0;
					be[i] = 0.0;
				}

				CElement* ele = pmeshele + num_ele - num_tet + i_tet;
				for (int i_p = 0; i_p < ap_elem; ++i_p) {
					x[i_p] = pmeshnode[ele->n[i_p]].x;
					y[i_p] = pmeshnode[ele->n[i_p]].y;
					z[i_p] = pmeshnode[ele->n[i_p]].z;
				}
                D = tet_Volume6(x, y, z);
				tet_Center(x, y, z, tetg);
				tet_SideLength(x, y, z, l);
				tetNedelec_Direction(x, y, z, si);
				/** 换一种计算方向的方法 **/
				//for (i = 0; i < mp_elem; ++i) {
				//	if (ele->n[tet_n[i][1]] < ele->n[tet_n[i][0]]) {
				//		si[i] = 1;
				//	} else {
				//		si[i] = -1;
				//	}
				//}
				tetNedelec_mk(x, y, z, xi, yi, zi);
				tetNedelec_XYZ(l, si, xi, yi, zi, X, Y, Z);
				tetNedelec_km_mk(x, y, z, xy, yz, zx);
				tetNedelec_Fxyz(l, si, tetg, xi, yi, zi, xy, yz, zx, Fx, Fy, Fz);
				tet_simple_b(x, y, z, b);
				tet_simple_c(x, y, z, c);
				tet_simple_d(x, y, z, d);

				/** 计算B **/
				CMaterial* mat = getMaterial(ele->geometry_tag);
				
				for (i = 0; i < mp_elem; i++) {
					xe[i] = A(ele->n[4 + i]);
				}
				/* |B[T]| */
				tetNedelec_rot(D, X, Y, Z, xe, rotA);
				B2 = rotA[0] * rotA[0] + rotA[1] * rotA[1] + rotA[2] * rotA[2];
				ele->B = sqrt(B2);ele->Bx = rotA[0];ele->By = rotA[1];ele->Bz = rotA[2];
				/** 处理非线性 **/
				if (mat->BHpoints) {
					LinearFlag = false;
					if (B2 > 0) {
						dnu = mat->getdvdB2(ele->B);
						nu = 1.0 / mat->getMiu(ele->B);
						double sr[mp_elem];
						for (i = 0; i < mp_elem; i++) {
							sr[i] = 0.0;
						}
						tet_MakeElement_NL_Mag_Static_Newton_A_SR_IP_1(ae, sr, xi, yi, zi, si, l, D, xe, B2, dnu);
						for (i = 0; i < mp_elem; i++) {
							bn(ele->n[i + 4]) += sr[i];
						}
					}					
				}
				nu = 1.0 / mat->getMiu(ele->B);
				/** 处理线圈 **/
				if (mat->coil) {
					for (i = 0; i < ap_elem; i++) {
						/** 计算顶点的电流密度大小和方向 **/
						for (j = 0; j < DIM; j++) {
                            Joe[i][j] = mat->coil->direction*pmeshnode[ele->n[i]].J[j] * mat->coil->Jor;
						}
					}
					tetNedelec_JoA(Joe, si, l, b, c, d, be_JoA);
					for (i = 0; i < mp_elem; i++) {
						bn(ele->n[i + 4]) += be_JoA[i];
					}
				}

				/** 处理永磁 **/
				if (mat->pm) {
					double a[ap_elem];
					double Me[ap_elem][DIM];
					double be_MrotA[mp_elem];

					tet_simple_a(x, y, z, a);

					for (i = 0; i < ap_elem; i++) {
						for (j = 0; j < DIM; j++, ii++)
							Me[i][j] = mat->pm->Hc * mat->pm->v[j];
					}
					tetNedelec_MrotA(Me, D, si, l, tetg, a, b, c, d, xi, yi, zi, be_MrotA);
					for (i = 0; i < mp_elem; i++) {
						bn(ele->n[i + 4]) += be_MrotA[i];
					}
				}
				/** 双旋度矩阵 **/
				tetNedelec_rotrot(D, X, Y, Z, rr);
				for (i = 0; i < mp_elem; i++) {
					ii = i * szRow;
					for (j = 0; j < mp_elem; j++, ii++) {
						ae[ii] += nu * rr[i][j];
					}
				}
                /** LM算子 **/
				tetNedelec_gradside(D, b, c, d, Fx, Fy, Fz, gs);
				/** 生成位置矩阵 **/
				int n_row, n_col;
				for (i = 0; i < mp_elem; i++) {
					n_row = node_pos(ele->n[i + 4]);
					ii = i * szRow;
					for (j = 0; j < mp_elem; j++, ii++) {
						n_col = node_pos(ele->n[j + 4]);
						if (n_row < num_dof && n_col < num_dof) {
                            insertSparseMatrixData(pos++, n_row, n_col, ae[ii]);
						}
					}
				}/** end of for **/
				for (i = 0; i < mp_elem; i++) {
					n_row = node_pos(ele->n[i + 4]);
					for (j = 0; j < ap_elem; j++) {
                        n_col = node_pos(num_edges + ele->n[j]);
						if (n_row < num_dof && n_col < num_dof) {
                            insertSparseMatrixData(pos++, n_row, n_col, gs[i][j]);
                            insertSparseMatrixData(pos++, n_col, n_row, gs[i][j]);
						}
					}
                }/** end of for **/
			}/** end of tet **/
			/** 装配 **/
			if (non_iter == 0) {
				locs.reshape(2, pos);
				vals.reshape(1, pos);
			}
			t0 = SuperLU_timer_();
			sp_mat X(true, locs, vals, num_dof, num_dof, true, true);
			//X.print_dense("X");
			for (int idof = 0; idof < edge_dof; ++idof) {
				unknown_b[idof] = bn(node_reorder(idof));
			}
			t1 = SuperLU_timer_();
			printf("Assembly time:%lf s\n", t1 - t0);
			printf("Sparse matrix info: row size:%d, col size:%d, non zero:%d\n", X.n_rows, X.n_cols, X.n_nonzero);
			
            if(0) {
                char fn[256];
                sprintf(fn, "%s%d%sunknown_b%d.txt", fileName, i_time, prefixName, non_iter);
                FILE *fp1 = fopen(fn, "w");
                for (int it = 0; it < num_dof; ++it) {
                    fprintf(fp1, "%lf\n", unknown_b[it]);
                }
                fclose(fp1);
            }
			/** 稀疏矩阵求解 **/
            //---------------------superLU_MT---------------------------------------
            printf("Solve linear sparse matrix using %d cores.\n", ParallelThreads);
            CSuperLU_MT superlumt(num_dof, X, unknown_b, ParallelThreads);
			if (superlumt.solve() == 1) {
				printf("Error: superlumt.slove. Info:%d\n", superlumt.info);
				return;
			} else {
				A_old = A;
				double* sol = superlumt.getResult();

                for (int idof = 0; idof < num_dof; ++idof) {
					A(node_reorder(idof)) = sol[idof];
				}
                if(0) {
                    char fn[256];
                    sprintf(fn, "%s%d%ssol%d.txt", fileName, i_time, prefixName, non_iter);
                    FILE *fp1 = fopen(fn, "w");
                    for (int it = 0; it < A.size(); ++it) {
                        fprintf(fp1, "%10.8e\n", A(it));
                    }
                    fclose(fp1);
                }
				/** 计算LM大小 **/
				double normp = 0.0;
                for (int iA = num_edges; iA < A.size(); ++iA) {
					normp += A(iA) * A(iA);
				}
				normp = sqrt(normp);
                /** 初始磁场可能为零 **/
                error = norm((A - A_old), 2);
                if(error > 1e-6) {
                    error /= norm(A_old, 2);
                }
				printf("error:%10.8e,normp:%10.8e\n", error, normp);
            }
            /** 更新磁场结果 **/
            t0 = SuperLU_timer_();
            printf("Solving time:%lf s\n", t0 - t1);
            SimulationTime[current_step] += t0-t2;
            if (error < Precision || LinearFlag) {
                IterationStep[current_step] = non_iter;
                break;
            }
			bn.zeros();
            pos = 0;
		}/** end of NR **/		
		
        /** 更新结果 **/

		/** 更新B **/
		updateResults();
		/** 计算电磁力 **/
        if(motion.type == MotionType::STRAIGHTMOTION)
            magforces[current_step]      = calcMagForce(tag_xiantie, FORCEMETHOD::MAXWELLTENSOR);
        else if(motion.type == MotionType::ROTATEMOTION)
            magforces[current_step]      = calcMagTorque(tag_xiantie, FORCEMETHOD::MAXWELLTENSOR);

		char fn[256];
        sprintf(fn, "%s%d%snoair.vtk", fileName, i_time, prefixName);
		writeVtkFileNoAir(fn);

        stepIncrement();
		/** 回收空间 **/
		if (unknown_b) {
			delete unknown_b;
			unknown_b = nullptr;
		}
    }
}



/*!
 * @brief      求解三维的涡流问题，默认电路当中只包含最简单的线圈、电阻、电源。
 * @author     Poofee
 * @date       2020年8月26日
 * @param[out] 
 * @return     void  
**/
void Relay3D::MagnetoDynamics3DNR() {
    double t0, t1, t2;
	t0 = SuperLU_timer_();
    double totalTime = 0;
    double Rc = 0;
    double Vc = 0;
	double icoil = 0;
	int timesize = timesteps.size();

    /** 求解静磁场进行初始化，因为可能有永磁等励磁源存在 **/

    /** 时间步迭代 **/
	for (int i_time = 1; i_time < timesize; ++i_time) {
        printCurrentTime();
        double dt = timesteps[i_time];
        totalTime += dt;
        totaltimes[i_time] = totalTime;
        printf("Time step %d/%d, current time is %lf s.\n", i_time, timesize, totalTime);
		/** remesh，要使用增量位移，向下为负，向上为正 **/
		remesh(i_time);

		/** 读取分网 **/
		loadMesh();
        unitConvert(unitRatio);

		/** 计算棱自由度 **/
		findBoundaryFaces3D(-1);
		findBoundaryEdges3D(-1);

        if(0) {
            char tmpfn[256];
            sprintf(tmpfn, "%s%sbnd.vtk", fileName, prefixName);
            testBoundaryResults(tmpfn);
            if (checkConnectivityMatrix()) {
                printf("Connectivity Matrix is right.\n");
            } else {
                printf("Connectivity Matrix is wrong.\n");
            }
        }
        findAir();
		makeJ();
        if(0) {
            char tmpfn[256];
            sprintf(tmpfn, "%s%sJ%d.vtk", fileName, prefixName, i_time);
            writeCurrentDensityField(tmpfn);
        }

        int edge_dof = num_edges - boundaryEdges.size();
		int num_dof = edge_dof + allPoints.size() - boundaryPoints.size();

        printf("DoF %d\n", num_dof+1);/** 包含电流 **/
        double* unknown_b = (double*)calloc(num_dof+1, sizeof(double));

        A_last = A;
        A_old = A;

		allocateMemory();
		setBoundary();

		double error;
		bn.zeros();
		bool LinearFlag = true;
		/** 非线性迭代求解 **/
        SimulationTime[current_step] = 0;
		for (int non_iter = 0; non_iter < MAX_NONLINEARSTEPS; ++non_iter) {
            t2 = SuperLU_timer_();
            printCurrentTime();
            printf("Time step (%d/%d)\tNewton's step (%d/%d)\n", i_time, timesize, non_iter, MAX_NONLINEARSTEPS);
			int pos = 0;
            unknown_b[num_dof] = 0;
            bn.zeros();
			for (int i_tet = 0; i_tet < num_tet; ++i_tet) {
				int    i, j, k, ii;
				double x[ap_elem], y[ap_elem], z[ap_elem];
				double D;
				double nu = 0;
				double tetg[ap_elem];
				double l[mp_elem];
				int    si[mp_elem];
				double xi[mp_elem], yi[mp_elem], zi[mp_elem];
				double xy[mp_elem], yz[mp_elem], zx[mp_elem];
				double X[mp_elem], Y[mp_elem], Z[mp_elem];
				double Fx[mp_elem], Fy[mp_elem], Fz[mp_elem];
				double b[ap_elem], c[ap_elem], d[ap_elem];
                double rr[mp_elem][mp_elem], gs[mp_elem][ap_elem];//, sg[ap_elem][mp_elem];
				//int    indRow[nd_elem];
				double ae[nd_elem*nd_elem];
				double fe[nd_elem];
				double rotA[DIM];
				double B2 = 0.0;
				int    szRow = nd_elem;
				double Joe[ap_elem][DIM];
				//double Ihe[ap_elem];

				double be_JoA[mp_elem];
				//double be_gs[mp_elem];
				double be[nd_elem];
				double dnu = 0.0;
				double xe[mp_elem];
                double xe_t[mp_elem];

				/** 初始化 **/
				for (i = 0, k = 0; i < nd_elem; i++) {
					for (j = 0; j < nd_elem; j++, k++)
						ae[k] = 0.0;
					fe[i] = 0.0;
					be[i] = 0.0;
				}

				CElement* ele = pmeshele + num_ele - num_tet + i_tet;
                /** 如果边界网格没变的话，应该是可以这样做的 **/
                CElement* ele_last = pmeshelelast + num_ele - num_tet + i_tet;
				for (int i_p = 0; i_p < ap_elem; ++i_p) {
					x[i_p] = pmeshnode[ele->n[i_p]].x;
					y[i_p] = pmeshnode[ele->n[i_p]].y;
					z[i_p] = pmeshnode[ele->n[i_p]].z;
				}
				D = tet_Volume6(x, y, z);
				tet_Center(x, y, z, tetg);
				tet_SideLength(x, y, z, l);
				tetNedelec_Direction(x, y, z, si);
				tetNedelec_mk(x, y, z, xi, yi, zi);
				tetNedelec_XYZ(l, si, xi, yi, zi, X, Y, Z);
				tetNedelec_km_mk(x, y, z, xy, yz, zx);
				tetNedelec_Fxyz(l, si, tetg, xi, yi, zi, xy, yz, zx, Fx, Fy, Fz);
				tet_simple_b(x, y, z, b);
				tet_simple_c(x, y, z, c);
				tet_simple_d(x, y, z, d);

				/** 计算B **/
				CMaterial* mat = getMaterial(ele->geometry_tag);

				for (i = 0; i < mp_elem; i++) {
					xe[i] = A(ele->n[4 + i]);
                    if(i_tet < air_position && i_time > 1) {
                        /** 貌似两次的棱边自由度的编号并不一致 **/
                        xe_t[i] = A_last(ele_last->n[4 + i]);
                        if(non_iter == 0) {
                            /** 设置A的初值 **/
                            xe[i] = xe_t[i];
                        }
                    }
				}
				/* |B[T]| */
				tetNedelec_rot(D, X, Y, Z, xe, rotA);
				B2 = rotA[0] * rotA[0] + rotA[1] * rotA[1] + rotA[2] * rotA[2];
				ele->B = sqrt(B2); ele->Bx = rotA[0]; ele->By = rotA[1]; ele->Bz = rotA[2];
				/** 处理非线性 **/
                if (mat->BHpoints) {
					LinearFlag = false;
                    if(B2 > 0) {
                        dnu = mat->getdvdB2(ele->B);
                        nu = 1.0 / mat->getMiu(ele->B);
                        double sr[mp_elem];
                        for (i = 0; i < mp_elem; i++) {
                            sr[i] = 0.0;
                        }
                        tet_MakeElement_NL_Mag_Static_Newton_A_SR_IP_1(ae, sr, xi, yi, zi, si, l, D, xe, B2, dnu);
                        for (i = 0; i < mp_elem; i++) {
                            bn(ele->n[i + 4]) += sr[i];
                        }
                    }
				}
				nu = 1.0 / mat->getMiu(ele->B);
                /** 双旋度矩阵 **/
                tetNedelec_rotrot(D, X, Y, Z, rr);
                for (i = 0; i < mp_elem; i++) {
                    ii = i * szRow;
                    for (j = 0; j < mp_elem; j++, ii++) {
                        ae[ii] += nu * rr[i][j];
                    }
                }
                /** 涡流 **/
                if(mat->sigma > 0 && i_time > 1 && i_tet < air_position) {
                    double ss[mp_elem][mp_elem] ;

                    double ee = mat->sigma / dt ;
                    tetNedelec_sideside( D, l, tetg, si, x, y, z, xi, yi, zi,
                                         xy, yz, zx, ss ) ;
                    for( i = 0 ; i < mp_elem ; i++ ) {
                        int    ii = i * szRow ;
                        for( j = 0 ; j < mp_elem ; j++,ii++ ) {
                            ae[ii] += ee * ss[i][j] ;
                        }
                    }

                    /** 上一时刻的影响 **/
                    for( i = 0 ; i < mp_elem ; i++ ) {
                        //xe_t[i] = A_last(ele_last->n[4 + i]);
                        for( j = 0 ; j < mp_elem ; j++ ) {
                            bn(ele->n[i + 4]) += ee * xe_t[j] * ss[i][j] ;
                        }
                    }
                }

                /** Lagrange multipliers算子 **/
                tetNedelec_gradside(D, b, c, d, Fx, Fy, Fz, gs);
				/** 处理线圈 **/
				if (mat->coil && i_time > 1) {
					Rc = mat->coil->Rc;
					Vc = mat->coil->Vc;
					for (i = 0; i < ap_elem; i++) {
						/** 计算顶点的电流密度大小和方向 **/
						for (j = 0; j < DIM; j++) {
                            Joe[i][j] = mat->coil->direction*pmeshnode[ele->n[i]].J[j];
                        }
					}
					tetNedelec_JoA(Joe, si, l, b, c, d, be_JoA);
					for (i = 0; i < mp_elem; i++) {
						int n_row = node_pos(ele->n[i + 4]);
						if (n_row < num_dof && i_time > 1) {
                            insertSparseMatrixData(pos++, n_row, num_dof, -be_JoA[i]* mat->tau);
                            insertSparseMatrixData(pos++, num_dof, n_row, -be_JoA[i] * mat->tau);
                            /** 最后一行 **/
                            xe_t[i] = A_last(ele_last->n[4 + i]);
                            unknown_b[num_dof] += -be_JoA[i] * mat->tau * xe_t[i];
						}
					}
				}

				/** 处理永磁 **/
				if (mat->pm) {
					double a[ap_elem];
					double Me[ap_elem][DIM];
					double be_MrotA[mp_elem];

					tet_simple_a(x, y, z, a);

					for (i = 0; i < ap_elem; i++) {
						for (j = 0; j < DIM; j++, ii++)
							Me[i][j] = mat->pm->Hc * mat->pm->v[j];
					}
					tetNedelec_MrotA(Me, D, si, l, tetg, a, b, c, d, xi, yi, zi, be_MrotA);
					for (i = 0; i < mp_elem; i++) {
						bn(ele->n[i + 4]) += be_MrotA[i];
					}
				}

				/** 生成位置矩阵 **/
				int n_row, n_col;
				for (i = 0; i < mp_elem; i++) {
					n_row = node_pos(ele->n[i + 4]);
					ii = i * szRow;
					for (j = 0; j < mp_elem; j++, ii++) {
						n_col = node_pos(ele->n[j + 4]);
						if (n_row < num_dof && n_col < num_dof) {
                            insertSparseMatrixData(pos++, n_row, n_col, ae[ii]);
						}
					}
				}/** end of for **/
				for (i = 0; i < mp_elem; i++) {
					n_row = node_pos(ele->n[i + 4]);
					for (j = 0; j < ap_elem; j++) {
                        n_col = node_pos(num_edges + ele->n[j]);
						if (n_row < num_dof && n_col < num_dof) {
                            insertSparseMatrixData(pos++, n_row, n_col, gs[i][j]);
                            insertSparseMatrixData(pos++, n_col, n_row, gs[i][j]);
						}
					}
				}/** end of for **/
                /**  non-conducting region **/
                if (i_time > 1 && mat->coil==nullptr && mat->sigma==0) {
					for (i = 0; i < ap_elem; i++) {
						n_row = node_pos(num_edges + ele->n[i]);
						for (j = 0; j < ap_elem; j++) {
							n_col = node_pos(num_edges + ele->n[j]);
							if (n_row < num_dof && n_col < num_dof) {
//                                insertSparseMatrixData(pos++, n_row, n_col, D / 120 * (1 + (i == j)));
							}
						}
					}/** end of for **/
				}
			}/** end of tet **/
			/** 右下角的点 **/
			if (i_time > 1) {
                insertSparseMatrixData(pos++, num_dof, num_dof, -dt*Rc);
			}
			/** 装配 **/
			if (non_iter == 0) {
				locs.reshape(2, pos);
				vals.reshape(1, pos);
			}
			t0 = SuperLU_timer_();
			int size = num_dof;
            if (i_time > 1){
                size += 1;
            }

			sp_mat X(true, locs, vals, size, size, true, true);
            if(0){
                char fn[256];
                sprintf(fn, "%s%d%sX%d.txt", fileName, i_time, prefixName, non_iter);
                std::ofstream xFile(fn);
                X.print(xFile);
            }

			for (int idof = 0; idof < edge_dof; ++idof) {
				unknown_b[idof] = bn(node_reorder(idof));
			}
            if (i_time > 1){
                unknown_b[num_dof] += -dt*Vc;
            }

			t1 = SuperLU_timer_();
			printf("Assembly time:%lf s\n", t1 - t0);
			printf("Sparse matrix info: row size:%d, col size:%d, non zero:%d\n", X.n_rows, X.n_cols, X.n_nonzero);

            if(0) {
                char fn[256];
                sprintf(fn, "%s%d%sunknown_b%d.txt", fileName, i_time, prefixName, non_iter);
                FILE *fp1 = fopen(fn, "w");
                for (int it = 0; it < size; ++it) {
                    fprintf(fp1, "%10.8e\n", unknown_b[it]);
                }
                fclose(fp1);
            }
            /** superLU_MT稀疏矩阵求解 **/
            CSuperLU_MT superlumt(size, X, unknown_b, ParallelThreads);
			if (superlumt.solve() == 1) {
				printf("Error: superlumt.slove. Info:%d\n", superlumt.info);
				return;
			} else {
				A_old = A;
				double* sol = superlumt.getResult();

                for (int idof = 0; idof < num_dof; ++idof) {
					A(node_reorder(idof)) = sol[idof];
				}
				if(i_time > 1)
					icoil = sol[num_dof];
                if(0) {
                    char fn[256];
                    sprintf(fn, "%s%d%ssol%d.txt", fileName, i_time, prefixName, non_iter);
                    FILE *fp1 = fopen(fn, "w");
                    for (int it = 0; it < A.size(); ++it) {
                        fprintf(fp1, "%10.8e\n", A(it));
                    }
                    fclose(fp1);
                }
				/** 计算LM大小 **/
				double normp = 0.0;
				for (int iA = num_edges; iA < A.size(); ++iA) {
					normp += A(iA) * A(iA);
				}
				normp = sqrt(normp);
                /** 初始磁场可能为零 **/
                error = norm((A - A_old), 2);
                if(error > 1e-6) {
                    error /= norm(A_old, 2);
                }
                printf("error:%10.8e,normp:%10.8e\n", error, normp);
            }
			bn.zeros();
			pos = 0;
			t0 = SuperLU_timer_();
            printf("Solving time:%lf s\n", t0 - t1);
            updateResults();
            printf("icoil:%10.8e,Phicoil:%10.8e\n", icoil, PhiCoil[current_step]);
            SimulationTime[current_step] += t0-t2;
            if (error < Precision || LinearFlag) {
                IterationStep[current_step] = non_iter;
                break;
            }
        }/** end of NR **/

		/** 更新B **/
		updateResults();

		/** 计算电磁力 **/
        if(motion.type == MotionType::STRAIGHTMOTION)
            magforces[current_step]      = calcMagForce(tag_xiantie, FORCEMETHOD::MAXWELLTENSOR);
        else if(motion.type == MotionType::ROTATEMOTION)
            magforces[current_step]      = calcMagTorque(tag_xiantie, FORCEMETHOD::MAXWELLTENSOR);
        /** 计算加速度->速度->位移 **/

        char fn[256];
        sprintf(fn, "%s%d%snoair.vtk", fileName, i_time, prefixName);
		writeVtkFileNoAir(fn);
        /** 更新结果 **/

		Ddisplacements[current_step] = 0;
		displacements[current_step]  = 0;
		velocities[current_step]     = 0;
		accelerations[current_step]  = 0;
		//PhiCoil[current_step]        = 0;
        ICoil[current_step]          = icoil;
		//magforces[current_step]      = 0;


        if(i_time > 1){
            printf("Phicoil:%10.8e\t", PhiCoil[current_step]);
            printf("Induced potential:%10.8e\n",(PhiCoil[current_step]-PhiCoil[current_step-1])/dt);
        }

        stepIncrement();

		/** 回收空间 **/
		if (unknown_b) {
			delete unknown_b;
			unknown_b = nullptr;
		}
	}
}

/*!
 * @brief      ATLM三维静态的实现。
 * @author     Poofee
 * @date       2020年8月30日
 * @param[out] 
 * @return     void  
**/
void Relay3D::MagnetoStatic3DATLM() {

}

/*!
 * @brief      QMTLM三维静态的实现。
 * @author     Poofee
 * @date       2020年10月31日
 * @param[out]
 * @return     void
**/
void Relay3D::MagnetoStatic3DQMTLM() {

}

/*!
 * @brief      ATLM三维瞬态的实现。
 * @author     Poofee
 * @date       2020年10月31日
 * @param[out]
 * @return     void
**/
void Relay3D::MagnetoDynamics3DATLM() {

}


/*!
 * @brief      生成三维vtk文件。
 * @author     Poofee
 * @date       2020年9月18日
 * @param[out] 
 * @param[in]  char * fname  
 * @return     void  
**/
void Relay3D::writeVtkFile(char* fname) {
	int num_cell = num_tet;
	int ele_size = 4;
	int ele_type = 10;
	Relay::writeVtkFile(fname, num_cell, ele_type, ele_size);

	FILE* fp;
	fp = fopen(fname, "a+");
	/** 数据 **/
	fprintf(fp, "\nPOINT_DATA %d\n", num_pts);
	fprintf(fp, "SCALARS %s double %d\n", "p", 1);
	fprintf(fp, "LOOKUP_TABLE %s\n", "ptable");
	for (int i = allEdges.size(); i < allEdges.size() + num_pts; ++i) {
		fprintf(fp, "%10.8e\n", A(i));
	}

	fprintf(fp, "\nCELL_DATA %d\n", num_cell);
	fprintf(fp, "SCALARS %s double %d\n", "B/T", 1);
	fprintf(fp, "LOOKUP_TABLE %s\n", "Btable");
	for (int i_tri = 0; i_tri < num_tet; ++i_tri) {
		int i1 = num_ele - num_tet + i_tri;
		fprintf(fp, "%10.8e\n", pmeshele[i1].B);
	}

	fprintf(fp, "\nVECTORS %s double\n", "Bvector");
	for (int i_tri = 0; i_tri < num_tet; ++i_tri) {
		int i1 = num_ele - num_tet + i_tri;
		fprintf(fp, "%10.8e %10.8e %10.8e\n", pmeshele[i1].Bx, pmeshele[i1].By, pmeshele[i1].Bz);
	}

	fclose(fp);
}


/*!
 * @brief      写出vtk文件，不包含空气，便于查看内部。
 * @author     Poofee
 * @date       2020年8月28日
 * @param[out] 
 * @param[in]  char * fname  
 * @return     void  
**/
void Relay3D::writeVtkFileNoAir(char * fname) {
	int num_cell = 0;
	int ele_size = 4;
	int ele_type = 10;
	/** 计算非空气单元数目 **/
	for (int i_tri = 0; i_tri < num_tet; ++i_tri) {
		int i1 = num_ele - num_tet + i_tri;
		if (pmeshele[i1].geometry_tag >= tag_air || pmeshele[i1].geometry_tag == tag_remesh) {
			continue;
		}
		/** 不导出线圈 **/
		CMaterial* mat = getMaterial(pmeshele[i1].geometry_tag);
		if (mat->tau > 0) {
			continue;
		}
		num_cell++;
	}

	FILE*  fp;
	fp = fopen(fname, "w");

	/*
	 1: points
	 3: line
	 5: Triangular element
	 9: Quadrilateral element
	10: Tetrahedral element
	12: Hexahedral element
	13: Triangular prism element
	14: Pyramid element
	*/
	/** 数据版本声明 **/
	fprintf(fp, "# vtk DataFile Version 2.0\n");
	/** 标题 **/
	fprintf(fp, "vtk title\n");
	/** 文件格式声明 **/
	fprintf(fp, "ASCII\n");
	/** 几何拓扑结构 **/
	fprintf(fp, "DATASET UNSTRUCTURED_GRID\n");
	/** 节点 **/
	fprintf(fp, "\nPOINTS %d double\n", num_pts);
	for (int i = 0; i < num_pts; ++i) {
		fprintf(fp, "%10.8e %10.8e %10.8e\n", pmeshnode[i].x, pmeshnode[i].y, pmeshnode[i].z);
	}
	/** 单元 **/
	fprintf(fp, "\nCELLS %d %d\n", num_cell, (ele_size + 1)*num_cell);
	for (int i_tri = 0; i_tri < num_tet; ++i_tri) {
		int i1 = num_ele - num_tet + i_tri;
		if (pmeshele[i1].geometry_tag >= tag_air || pmeshele[i1].geometry_tag == tag_remesh) {
			continue;
		}
		/** 不导出线圈 **/
		CMaterial* mat = getMaterial(pmeshele[i1].geometry_tag);
		if (mat->tau > 0) {
			continue;
		}
		fprintf(fp, "%d", ele_size);
		for (int i = 0; i < ele_size; ++i) {
			fprintf(fp, " %d", pmeshele[i1].n[i]);
		}
		fprintf(fp, "\n");
	}
	/** 单元类型 **/
	fprintf(fp, "\nCELL_TYPES %d\n", num_cell);
	for (int i = 0; i < num_tet; ++i) {
		int i1 = num_ele - num_tet + i;
		if (pmeshele[i1].geometry_tag >= tag_air || pmeshele[i1].geometry_tag == tag_remesh) {
			continue;
		}
		/** 不导出线圈 **/
		CMaterial* mat = getMaterial(pmeshele[i1].geometry_tag);
		if (mat->tau > 0) {
			continue;
		}
		fprintf(fp, "%d\n", ele_type);
	}

	/** 数据 **/
	fprintf(fp, "\nPOINT_DATA %d\n", num_pts);
	fprintf(fp, "SCALARS %s double %d\n", "p", 1);
	fprintf(fp, "LOOKUP_TABLE %s\n", "ptable");
	for (int i = allEdges.size(); i < allEdges.size() + num_pts; ++i) {
		fprintf(fp, "%10.8e\n", A(i));
	}

	/** 写电磁力 **/
	fprintf(fp, "\nSCALARS %s double %d\n", "F/N", 1);
	fprintf(fp, "LOOKUP_TABLE %s\n", "FTable");
	for (int i = 0; i < num_pts; ++i) {
		double f = 0;
		for (int k = 0; k < 3; k++) {
			f += pmeshnode[i].F[k] * pmeshnode[i].F[k];
		}
		f = sqrt(f);
		fprintf(fp, "%10.8e\n", f);
	}

	/** 写电磁力 **/
	fprintf(fp, "\nVECTORS %s double\n", "FVector");
	for (int i = 0; i < num_pts; ++i) {
		fprintf(fp, "%10.8e %10.8e %10.8e\n", pmeshnode[i].F[0], pmeshnode[i].F[1], pmeshnode[i].F[2]);
	}

	/** 写磁感应强度 **/
	fprintf(fp, "\nCELL_DATA %d\n", num_cell);
	fprintf(fp, "SCALARS %s double %d\n", "B/T", 1);
	fprintf(fp, "LOOKUP_TABLE %s\n", "Btable");
	for (int i_tri = 0; i_tri < num_tet; ++i_tri) {
		int i1 = num_ele - num_tet + i_tri;
		if (pmeshele[i1].geometry_tag >= tag_air || pmeshele[i1].geometry_tag == tag_remesh) {
			continue;
		}
		/** 不导出线圈 **/
		CMaterial* mat = getMaterial(pmeshele[i1].geometry_tag);
		if (mat->tau > 0) {
			continue;
		}
		fprintf(fp, "%10.8e\n", pmeshele[i1].B);
	}

	fprintf(fp, "\nVECTORS %s double\n", "Bvector");
	for (int i_tri = 0; i_tri < num_tet; ++i_tri) {
		int i1 = num_ele - num_tet + i_tri;
		if (pmeshele[i1].geometry_tag >= tag_air || pmeshele[i1].geometry_tag == tag_remesh) {
			continue;
		}
		/** 不导出线圈 **/
		CMaterial* mat = getMaterial(pmeshele[i1].geometry_tag);
		if (mat->tau > 0) {
			continue;
		}
		fprintf(fp, "%10.8e %10.8e %10.8e\n", pmeshele[i1].Bx, pmeshele[i1].By, pmeshele[i1].Bz);
	}

	fclose(fp);
}


/*!
 * @brief      生成电流密度的vtk文件。
 * @author     Poofee
 * @date       2020年9月18日
 * @param[out] 
 * @param[in]  char * fname  
 * @return     void  
**/
void Relay3D::writeCurrentDensityField(char* fname) {
	int num_cell = num_tet;
	int ele_size = 4;
	int ele_type = 10;
	Relay::writeVtkFile(fname, num_cell, ele_type, ele_size);

	FILE* fp;
	fp = fopen(fname, "a+");
	/** 数据 **/
	fprintf(fp, "\nPOINT_DATA %d\n", num_pts);
	fprintf(fp, "SCALARS %s double %d\n", "J", 1);
	fprintf(fp, "LOOKUP_TABLE %s\n", "Jtable");
	for (int i = 0; i < num_pts; ++i) {
		double j = 0;
		for (int k = 0; k < 3; k++) {
			j += pmeshnode[i].J[k] * pmeshnode[i].J[k];
		}
		j = sqrt(j);
		fprintf(fp, "%10.8e\n", j);
	}

	fprintf(fp, "\nVECTORS %s double\n", "Jvector");
	for (int i = 0; i < num_pts; ++i) {
		fprintf(fp, "%10.8e %10.8e %10.8e\n", pmeshnode[i].J[0], pmeshnode[i].J[1], pmeshnode[i].J[2]);
    }
	fclose(fp);
}



/*!
 * @brief      
 * @author     Poofee
 * @date       2020年9月18日
 * @param[out] 
 * @param[in]  char * fname  
 * @return     void  
**/
void Relay3D::writeNodalForceDensityField(char* fname) {
    int num_cell = num_tet;
    int ele_size = 4;
    int ele_type = 10;
    Relay::writeVtkFile(fname, num_cell, ele_type, ele_size);

    FILE* fp;
    fp = fopen(fname, "a+");
    /** 数据 **/
    fprintf(fp, "\nPOINT_DATA %d\n", num_pts);
    fprintf(fp, "SCALARS %s double %d\n", "J", 1);
    fprintf(fp, "LOOKUP_TABLE %s\n", "FTable");
    for (int i = 0; i < num_pts; ++i) {
        double f = 0;
        for (int k = 0; k < 3; k++) {
            f += pmeshnode[i].F[k] * pmeshnode[i].F[k];
        }
        f = sqrt(f);
        fprintf(fp, "%10.8e\n", f);
    }

    fprintf(fp, "\nVECTORS %s double\n", "FVector");
    for (int i = 0; i < num_pts; ++i) {
        fprintf(fp, "%10.8e %10.8e %10.8e\n", pmeshnode[i].F[0], pmeshnode[i].F[1], pmeshnode[i].F[2]);
    }
	fclose(fp);
}


/*!
 * @brief      计算index区域的电磁力
 * @author     Poofee
 * @date       2020年9月18日
 * @param[out] 
 * @param[in]  int index  
 * @return     void  
**/
double Relay3D::calcMagForce(int index, FORCEMETHOD method) {
    double totalForce = 0;
    if(method == FORCEMETHOD::VIRTUALWORK){
        totalForce = nodalForceVW(index);
    }
    if(method == FORCEMETHOD::MAXWELLTENSOR){
        totalForce = nodalForceMST(index);
    }
    return totalForce;
}


/*!
 * @brief      计算电磁力，使用Maxwell's stress tensor
 * @author     Poofee
 * @date       2020年9月18日
 * @param[out] 
 * @param[in]  int index  
 * @return     void  
**/
double Relay3D::nodalForceMST(int index) {
    if (index < 0) {
        index = tag_xiantie;
    }

    double dd_6into1 = 1.0 / 6.0 ;
    double totalForce = 0;
    double totalForceX = 0;
    double totalForceY = 0;
    double totalForceZ = 0;
    for (int i = 0; i < num_pts; ++i) {
        pmeshnode[i].F[0] = 0.0;
        pmeshnode[i].F[1] = 0.0;
        pmeshnode[i].F[2] = 0.0;
    }

    /** 计算组件index上的电磁力 **/
    int * indexFlag = (int*)malloc(num_pts * sizeof(int));
    int MOVED = 1;
    for (int i = 0; i < num_pts; ++i) {
        indexFlag[i] = -1;
    }
    for (int i_tet = 0; i_tet < num_tet; ++i_tet) {
        CElement* ele = pmeshele + num_ele - num_tet + i_tet;

        if (ele->geometry_tag == index) {
            for (int i = 0; i < ap_elem; ++i) {
                indexFlag[ele->n[i]] = MOVED;
            }
        }
    }

    for (int i_tet = 0; i_tet < num_tet; ++i_tet) {
        CElement* ele = pmeshele + num_ele - num_tet + i_tet;
        CMaterial* mat = getMaterial(ele->geometry_tag);

        double Be[3] ;
        double nu = 0.0 ;

        double x[ap_elem], y[ap_elem], z[ap_elem] ;
        double b[ap_elem], c[ap_elem], d[ap_elem] ;

        /** Lower triangular matrix of T (Maxwell's stress tensor) **/
        double T_L[6] ;

		//if (ele->geometry_tag == index) continue;

        Be[0] = ele->Bx; Be[1] = ele->By; Be[2] = ele->Bz;
        nu = 1.0 / mat->getMiu(ele->B);

        for (int i_p = 0; i_p < ap_elem; ++i_p) {
            x[i_p] = pmeshnode[ele->n[i_p]].x;
            y[i_p] = pmeshnode[ele->n[i_p]].y;
            z[i_p] = pmeshnode[ele->n[i_p]].z;
        }
        tet_simple_b( x, y, z, b ) ;
        tet_simple_c( x, y, z, c ) ;
        tet_simple_d( x, y, z, d ) ;

        T_L[0] = nu * 0.5 * (Be[0]*Be[0] - Be[1]*Be[1] - Be[2]*Be[2]) ;
        T_L[1] = nu * Be[1] * Be[0] ;
        T_L[2] = nu * 0.5 * (Be[1]*Be[1] - Be[2]*Be[2] - Be[0]*Be[0]) ;
        T_L[3] = nu * Be[2] * Be[0] ;
        T_L[4] = nu * Be[2] * Be[1] ;
        T_L[5] = nu * 0.5 * (Be[2]*Be[2] - Be[0]*Be[0] - Be[1]*Be[1]) ;

		for (int i = 0; i < ap_elem; ++i) {
            int p = ele->n[i];
            if(indexFlag[p] == MOVED) {
                pmeshnode[p].F[0] -= dd_6into1 * (b[i] * T_L[0] + c[i] * T_L[1] + d[i] * T_L[3]);
                pmeshnode[p].F[1] -= dd_6into1 * (b[i] * T_L[1] + c[i] * T_L[2] + d[i] * T_L[4]);
                pmeshnode[p].F[2] -= dd_6into1 * (b[i] * T_L[3] + c[i] * T_L[4] + d[i] * T_L[5]);
            }
		}
    }


	for (int i = 0; i < num_pts; ++i) {
        if (indexFlag[i] == MOVED) {
            totalForceX += pmeshnode[i].F[0];
            totalForceY += pmeshnode[i].F[1];
            totalForceZ += pmeshnode[i].F[2];
		}
	}
	free(indexFlag);

    totalForce = totalForceX * totalForceX + totalForceY * totalForceY + totalForceZ * totalForceZ;
    totalForce = sqrt(totalForce);
    printf("Force (Maxwell's Stress Tensor) in domain %d:%10.8e N(%10.8e,%10.8e,%10.8e) N\n",index,totalForce,totalForceX,totalForceY,totalForceZ);
	return totalForce;
}


/*!
 * @brief      计算电磁力，虚功法
 * @author     Poofee
 * @date       2020年9月18日
 * @param[out] 
 * @param[in]  int index  
 * @return     void  
**/
double Relay3D::nodalForceVW(int index) {
    double totalForce = 0;
    double totalForceX = 0;
    double totalForceY = 0;
    double totalForceZ = 0;

    for (int i = 0; i < num_pts; ++i) {
        pmeshnode[i].F[0] = 0.0;
        pmeshnode[i].F[1] = 0.0;
        pmeshnode[i].F[2] = 0.0;
    }

    for (int i_tet = 0; i_tet < num_tet; ++i_tet) {
        CElement* ele = pmeshele + num_ele - num_tet + i_tet;
        CMaterial* mat = getMaterial(ele->geometry_tag);

        if(ele->geometry_tag != index) continue;

        double Be[3] ;
        double nu = 0.0 ;

        double x[ap_elem], y[ap_elem], z[ap_elem] ;
        double b[ap_elem], c[ap_elem], d[ap_elem] ;

        Be[0] = ele->Bx; Be[1] = ele->By; Be[2] = ele->Bz;
        nu = 1.0 / mat->getMiu(ele->B);

        for (int i_p = 0; i_p < ap_elem; ++i_p) {
            x[i_p] = pmeshnode[ele->n[i_p]].x;
            y[i_p] = pmeshnode[ele->n[i_p]].y;
            z[i_p] = pmeshnode[ele->n[i_p]].z;
        }
        tet_simple_b( x, y, z, b ) ;
        tet_simple_c( x, y, z, c ) ;
        tet_simple_d( x, y, z, d ) ;


        for(int i = 0 ; i < ap_elem ; ++i) {
            totalForceX -= 0 ;
            totalForceY -= 0 ;
            totalForceZ -= 0 ;

            pmeshnode[ele->n[i]].F[0] -= 0 ;
            pmeshnode[ele->n[i]].F[1] -= 0 ;
            pmeshnode[ele->n[i]].F[2] -= 0 ;
        }
    }
    totalForce = totalForceX * totalForceX + totalForceY * totalForceY + totalForceZ * totalForceZ;
    totalForce = sqrt(totalForce);
    printf("Force (Virtual Work) in domain %d:%10.8e N(%10.8e,%10.8e,%10.8e) N\n",index,totalForce,totalForceX,totalForceY,totalForceZ);
	return totalForce;
}



/*!
 * @brief      计算index区域的电磁力矩
 * @author     Poofee
 * @date       2020年9月18日
 * @param[out] 
 * @param[in]  int index  
 * @return     void  
**/
double Relay3D::calcMagTorque(int index, FORCEMETHOD method) {
    double totalTorque = 0;
    double totalTorqueX = 0;
    double totalTorqueY = 0;
    double totalTorqueZ = 0;

    /** 计算节点力 **/
    if(method == FORCEMETHOD::VIRTUALWORK){
        totalTorque = nodalForceVW(index);
    }
    if(method == FORCEMETHOD::MAXWELLTENSOR){
        totalTorque = nodalForceMST(index);
    }
    /** 力矩 **/
    double L[3];
    double k;
    double M[3];
    for(int i = 0; i < num_pts; ++i) {
        /** 计算力臂 **/
        L[0] = pmeshnode[i].x - motion.p[0]*unitRatio;
        L[1] = pmeshnode[i].y - motion.p[1]*unitRatio;
        L[2] = pmeshnode[i].z - motion.p[2]*unitRatio;
        k = (motion.v[0]*L[0]+motion.v[1]*L[1]+motion.v[2]*L[2])/(motion.v[0]*motion.v[0]+motion.v[1]*motion.v[1]+motion.v[2]*motion.v[2]);
        L[0] -= k*motion.v[0];
        L[1] -= k*motion.v[1];
        L[2] -= k*motion.v[2];
        /** 计算力矩 **/
        M[0] = L[1]*pmeshnode[i].F[2]-L[2]*pmeshnode[i].F[1];
        M[1] = L[2]*pmeshnode[i].F[0]-L[0]*pmeshnode[i].F[2];
        M[2] = L[0]*pmeshnode[i].F[1]-L[1]*pmeshnode[i].F[0];

        totalTorqueX += M[0];
        totalTorqueY += M[1];
        totalTorqueZ += M[2];
    }

    totalTorque = totalTorqueX * totalTorqueX + totalTorqueY * totalTorqueY + totalTorqueZ * totalTorqueZ;
    totalTorque = sqrt(totalTorque);
    printf("Torque in domain %d:%10.8e N.m(%10.8e,%10.8e,%10.8e) N.m\n",index,totalTorque,totalTorqueX,totalTorqueY,totalTorqueZ);

    return totalTorque;
}


/*!
 * @brief      求解完后更新结果。
 * @author     Poofee
 * @date       2020年9月12日
 * @param[out] 
 * @return     void  
**/
void Relay3D::updateResults() {
	double phicoil = 0;
	for (int i_tet = 0; i_tet < num_tet; ++i_tet) {
		int    i, j, k, ii;
		double x[ap_elem], y[ap_elem], z[ap_elem];
		double D;
		double tetg[ap_elem];
		double l[mp_elem];
		int    si[mp_elem];
		double xi[mp_elem], yi[mp_elem], zi[mp_elem];
		double xy[mp_elem], yz[mp_elem], zx[mp_elem];
		double b[ap_elem], c[ap_elem], d[ap_elem];
		double X[mp_elem], Y[mp_elem], Z[mp_elem];
		double rotA[DIM];
		double B2 = 0.0;
		double xe[mp_elem];
		double Joe[ap_elem][DIM];
		double be_JoA[mp_elem];

		CElement* ele = pmeshele + num_ele - num_tet + i_tet;
		for (int i_p = 0; i_p < ap_elem; ++i_p) {
			x[i_p] = pmeshnode[ele->n[i_p]].x;
			y[i_p] = pmeshnode[ele->n[i_p]].y;
			z[i_p] = pmeshnode[ele->n[i_p]].z;
		}
		D = tet_Volume6(x, y, z);
		tet_SideLength(x, y, z, l);
		tetNedelec_Direction(x, y, z, si);
		//for (i = 0; i < mp_elem; ++i) {
		//	if (ele->n[tet_n[i][1]] < ele->n[tet_n[i][0]]) {
		//		si[i] = 1;
		//	} else {
		//		si[i] = -1;
		//	}
		//}
		tetNedelec_mk(x, y, z, xi, yi, zi);
		tetNedelec_XYZ(l, si, xi, yi, zi, X, Y, Z);

		/** 计算B **/
		CMaterial* mat = getMaterial(ele->geometry_tag);

		for (i = 0; i < mp_elem; i++) {
			xe[i] = A(ele->n[4 + i]);
		}
		/* |B[T]| */
		tetNedelec_rot(D, X, Y, Z, xe, rotA);
		B2 = rotA[0] * rotA[0] + rotA[1] * rotA[1] + rotA[2] * rotA[2];
		ele->B = sqrt(B2); ele->Bx = rotA[0]; ele->By = rotA[1]; ele->Bz = rotA[2];
		/** 计算线圈磁通 **/
		if (mat->coil) {
			for (i = 0; i < ap_elem; i++) {
				/** 计算顶点的电流密度大小和方向 **/
				for (j = 0; j < DIM; j++) {
                    Joe[i][j] = pmeshnode[ele->n[i]].J[j];
				}
			}
			tet_simple_b(x, y, z, b);
			tet_simple_c(x, y, z, c);
			tet_simple_d(x, y, z, d);
			tetNedelec_JoA(Joe, si, l, b, c, d, be_JoA);
			for (i = 0; i < mp_elem; i++) {
                phicoil += be_JoA[i] * mat->tau * xe[i];
			}

		}
	}
	PhiCoil[current_step] = phicoil;
}

/*!
 * @brief      
 * @author     Poofee
 * @date       2020年10月24日
 * @param[out] 
 * @return     void  
**/
void Relay3D::StaticInitial() {
	printf("Please finish it in subclass: void Relay3D::StaticInitial()\n");
}

/*!
 * @brief      
 * @author     Poofee
 * @date       2020年10月24日
 * @param[out] 
 * @return     void  
**/
void Relay3D::TransientInitial() {
	printf("Please finish it in subclass: void Relay3D::TransientInitial()\n");
}

/*!
 * @brief      静态问题分析
 * @author     Poofee
 * @date       2020年10月24日
 * @param[out] 
 * @return     void  
**/
void Relay3D::StaticAnalysis() {
	printf("Start Relay3D Static Analysis\n");
	StaticInitial();
	openGeo();
    MagnetoStatic3DNR();
	/** 输出信息 **/
	outputResults();
}

/*!
 * @brief      瞬态问题分析
 * @author     Poofee
 * @date       2020年10月24日
 * @param[out] 
 * @return     void  
**/
void Relay3D::TransientAnalysis() {
	printf("Start Relay3D Transient Analysis\n");
	TransientInitial();
    openGeo();
    MagnetoDynamics3DNR();
    outputResults();
}
